%作业6_8-test

%% 准备工作
simu_period = 1e-4;
simu_step = t_ed/simu_period;
sample_period = simu_period* 1;
n_group = 1;

motor_res = zeros(n_group,length(1:simu_step),3);
state_info = zeros(n_group,length(1:simu_step),3);
debug_info = zeros(n_group,length(1:simu_step),4);


step_value = -pi/2;
initial_state = [5*pi/180-pi/2 0 0];
is_g_enable = 1;
n_factor = 1;
err = 0.1;
a_sj = finalSingleJoint(initial_state,is_g_enable,n_factor);
a_sj.update_m_l(1-err);

a_sj.reset_state(initial_state);

%ts = 1, zeta = 1
inv_dynamic_con = newPidControler(a_sj,tor_motor,0.5,1,1,1);
%先把pid架设好
con_value = 0;
sample_time_count = sample_period;

%计算理论的稳态值
% equ = @(th)(th-(a_sj.m*a_sj.g*a_sj.l - a_sj.f_m*a_sj.g*a_sj.f_l)*sin(th)...
%     /(a_sj.n_factor*a_sj.f_Im*inv_dynamic_con.pid_kp));
% 
% th_ess = fsolve(equ,5*pi/180)


%% loop

for i = 1:simu_step
    if(sample_time_count >= sample_period)
        inv_dynamic_con.set_target([step_value 0 0]);
        if(i == simu_step-3000)
           disp("a"); 
        end
        debug_info(:,i,:) =  inv_dynamic_con.get_id_con_value(a_sj,tor_motor,sample_period);
        con_value = debug_info(:,i,1);
        sample_time_count = sample_time_count-sample_period;
    else
        debug_info(:,i,:) = debug_info(:,i-1,:);
    end
    motor_res(:,i,:) = tor_motor.simu_loop(a_sj,simu_period,con_value);
    state_info(:,i,:) = (a_sj.simu_loop(motor_res(:,i,1),simu_period));
    sample_time_count = sample_time_count+simu_period;
end


%%



%绘图
figure;
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
yline(step_value,'r--');
legend('n=10','target')
title("pos hold  theta");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,2),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
yline(0,'r--');
legend('n=10','target')
title("pos hold  omega");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,3),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
yline(0,'r--');
legend('n=10','target')
title("pos hold  acc");